#ifndef COMMANDS_H_
#define COMMANDS_H_

#include <config.h>
#include "parse.h"
#include "servos.h"
#include "vertify.h"
#include "motors.h"
#include "comp_com.h"

int command_setup(){
  comp_com_init();
  motor_init();
  servo_init();
}

// !!!!!! There is another forward declartion in comp_com
int command(String inputString, char** output){
  char comps[NUM_COMMAND_COMS];
  //  send_to_comp(inputString);  
  if(getReal(inputString, comps) < 0){
    Serial.println("ERROR:: In command, was unable to get real comps.");
    return -1;
  }

  /*  for(int i = 0; i < NUM_COMMAND_COMS; ++i){
    int intForm = (int)comps[i];
    Serial.println(intForm);
    }*/

  int errors = 0;
  if(commandMotors(comps) < 0){
    Serial.println("ERROR:: In command, was unable to command motors.");
    errors = 1;
  }

  if(commandServos(comps) < 0){
    Serial.println("ERROR: In command, was unable to command arm.");
    errors = 1;
  }

  if(errors != 0){
    Serial.println("ERROR: In command, some commanding errors occured.");
    return -1;
  }
  
  return 0;
}
#endif
